// HC-SR04 Using gpiod
#include <gpiod.h>
#include <error.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <iostream>
#include <chrono>
#include "hc_sr04.h"

using namespace std;

HCSR04::HCSR04()
{

}

void HCSR04::init(int trigger, int echo)
{
    this->trigger=trigger;
    this->echo=echo;

    this->chip = gpiod_chip_open("/dev/gpiochip0");
    //trigger
    this->trigger_line=gpiod_chip_get_line(this->chip,this->trigger);
    int ret=gpiod_line_request_output(this->trigger_line,"trigger",0);
    //>echo
    this->echo_line=gpiod_chip_get_line(this->chip, this->echo);
    ret=gpiod_line_request_input(this->echo_line,"echo");
    usleep(500*1000); //
}

double HCSR04::distance(int timeout)
{
    usleep(10*1000);

    gpiod_line_set_value(this->trigger_line,1);
    usleep(10); // A minimum period if 10us is needed to trigger a measurement - microseconds 微秒; 百万分之一秒
    gpiod_line_set_value(this->trigger_line,0);
   
     auto now= chrono::high_resolution_clock::now();
     while (gpiod_line_get_value(this->echo_line) == 0 &&  chrono::duration_cast<chrono::microseconds>(chrono::high_resolution_clock::now() -now).count()<timeout);
        recordPulseLength();
     
    distanceMeters = 100*(( chrono::duration_cast<chrono::microseconds>(endTimeUsec-startTimeUsec).count()/1000000.0)*340.29)/2;
    return distanceMeters;
}

void HCSR04::recordPulseLength()
{
    startTimeUsec = chrono::high_resolution_clock::now();
    while (gpiod_line_get_value(this->echo_line) == 1 );
    endTimeUsec = chrono::high_resolution_clock::now();
}

HCSR04::~HCSR04()
{
    gpiod_line_release(this->trigger_line);
    gpiod_line_release(this->echo_line);
    gpiod_chip_close(this->chip);    
}


